/**
*@class Drivecontrol
*@brief makes sure maneuvers are executed correctly (feedback)
*@author Elias
*@date 04-2011
* 
* @section source Sources
* AI, IR, MouseCC, Motion
*
* @section working What does the code
* The code makes sure maneuvers are executed correctly using feedback from the mice.
*
* @section destination Destination
* Motion, MouseCC
*
*/

#ifndef DRIVECONTROL_H
#define DRIVECONTROL_H

//This class arranges the "drive on a strait line" feedback.
//It receives a basic driving command like in accelerate or turn and
//makes sure its done correctly

#include <time.h>
#include "motor.h"
#include "mousecc.h"
#include "log.h"

//class MouseCC; //let the compiler know there will be a class called MouseCC, but isn't this defined in the mousecc.h????

enum drivecontrolState {stop, forward, backward, rotateR, rotateL, feedbackless};


class Drivecontrol
{
public:

	//we use this function to induce a change in the current driving style
	//drive on a strait line, acceleration is handled by the Trex board

	/**
	*@brief initDriveForward
	*@param the target speed
	*returns after rotation
	*/
	int initDriveForward( short targetSpeed );
	
	/**
	*@brief initDriveBackward
	*@param the target speed
	*returns after rotation
	*/
	int initDriveBackward( short targetSpeed );
	
	/**
	 * @brief stopManeuver
	 * stops whatever the robot is doing
	 */
	void stopManeuver();

	/**
	*@brief initRotate
	*@param rotates with this angle (counter clock wise is positive)
	*@param wait for end of maneuver if set
	*/
	int initRotate( float targetAngle, bool awaitEnd );
	

	/**
	*@brief feedback
	*@param the current angle of the robot (counter clock wise is positive)
	* function is ment to be called by the mouse feedback thread
	*/
	void feedback( float angle );

	/**
	*@brief testDrive
	*@param speed
	*@param duration of test
	*/
	int testDrive( short speed, short seconds );

	/**
	*@brief testRotation
	*@param speed
	*@param duration of test
	*/
	int testRotation( short speed, short seconds );

	/**
	*@brief getState
	*for Motion to make the next sane desicion
	*/
	drivecontrolState getState(void);
	
	/**
	*@brief getSpeed
	*why not
	*/
	short getSpeed(void);

	/**
	*@brief corrFactor
	*this value is added/substracted to both motors when the rotationTrigger is crossed
	*/
	int corrFactor_;

	/**
	*@brief rotationTrigger
	*this is the value of rotation we want to react on for motor adjustments
	*/
	float rotationTrigger_;
	bool useFeedback_;

	/**
	 * @brief timespec rec_
	 * Time delay we want for every wait cycle in initRotate.
	 * default: 1/20 of a second
	 */
	struct timespec rec_;

	/**
	 * @brief rotationSpeed
	 * @sa rotationDelay
	 * the speed at which the robot rotates
	 */
	int rotationSpeed_;

	/**
	 * @brief rotationDelay
	 * the time needed to make a 5 degrees rotation
	 * With this i hope to rotate without using feedback
	 *
	 */
	struct timespec rotationDelay_;

	Drivecontrol(bool useFeedback );
	~Drivecontrol();

private:

	//feedback functions
	//correction
	void driveCorr( float angle );
	//rotate right
	void rr ( float angle );
	//rotate left
	void rl ( float angle );

	//here are all the possible the arguments the by-feedback-called functions uses
	//there is only one kind of driving style at the same time possible
	short targetSpeed_;
	short targetAngle_;
    float totalAngle_;
	//some other vars
	short speed_, speedL_, speedR_;
	drivecontrolState state_;

	//classes
	Motor motors;
    Log *log;
    //this gave me compiler errors. I have solved them by making sure mousecc.cpp is compiled before drivecontrol.cpp.
    //maybe someone knows a better way?
    MouseCC mice;

};

#endif //DRIVECONTROL_H
